import rospy
from std_msgs.msg import Float32MultiArray,UInt8
from six_dof_arm import SixDofArm
from protobot.webots import Robot

arm = SixDofArm()
arm_name = arm._robot.getName()
arm.enable()
arm.home()
timestep = int(arm._robot.getBasicTimeStep())

def set_arm_position(data):
    arm.target_pos = data.data[0:3]
    arm.inverse_kinemetics(data.data[3])
    if len(data.data)==5:
        arm.delay(data.data[4])

def set_gripple(data):
    arm.target_gripple = data.data
    arm.set_hand()

rospy.init_node(arm_name)
rospy.Subscriber("~set_arm_position",Float32MultiArray,callback=set_arm_position)
rospy.Subscriber("~set_gripple",UInt8,callback=set_gripple)

if __name__ == "__main__":
    while arm._robot.step(timestep) != -1 and not rospy.is_shutdown():
        #if arm_name == 'sixdof_1':
            #print(arm_name,arm.forward_kinemetics(),sep=".")
        pass